/*
 * Motors.c
 *
 *  Created on: Jan 27, 2025
 *      Author: hzy
 */
#include "debug.h"
#include "Operator.h"

void InitMotors()
{
    // Initialize PortD
    GPIO_InitTypeDef GPIO_InitStructure = {0};

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_2 | GPIO_Pin_3;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOD, &GPIO_InitStructure);

    // Initialize PortC
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOC, &GPIO_InitStructure);

    // Enable All Motors
    GPIO_WriteBit(GPIOD, GPIO_Pin_4, 1);

    // MotorA
    OperatorStop();
}

//void InitMotors()
//{
//    // MotorA
//
//
//    // MotorB
//
//    //MotorC
//
//    //MotorD
//
//}
